/*
 * Copyright (c) 2011-2012 Johann Hanne
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/select.h>
#include <unistd.h>
#include <sys/time.h>
#include <stdarg.h>

#include "serport.h"

#define DEFAULT_SERPORTDEV "/dev/ttyUSB0"

/*
BA 18 FF                  mov         dx,0xff18 
B8 0E 00                  mov         ax,0x000e 
EF                        out         dx,ax 
CD FF                     int         0xff 
*/
static uint8_t set_uart1_divisor_code[] = {
    0xBA, 0x18, 0xFF,
    0xB8, 0x0E, 0x00,
    0xEF,
    0xCD, 0xFF
};

static int progressline_active = 0;

static void print_progressline(unsigned long int current, unsigned long int full, struct timeval *starttime) {
    int i;
    struct timeval curtime;
    unsigned long int elapsed;
    unsigned long int eta;

    progressline_active = 1;

    printf("%3lu%% [", (current == full) ? 100 : (current / ((full + 99) / 100)));

    for (i = 0; i < 50; i++) {
        printf("%c", ((current / (full / 50)) > i) ? '=' : ' ');
    }

    printf("]");

    if (gettimeofday(&curtime, NULL) == 0) {
        elapsed = (curtime.tv_sec - starttime->tv_sec) * 1000ul;
        elapsed += (curtime.tv_usec - starttime->tv_usec) / 1000;
        if (elapsed > 1000ul && (current / (full / 100)) > 0) {
            eta = (((full - current) / 100) * elapsed) / (current / 100);
            printf(" ETA: %lum%02lu", eta / 60000ul, (eta % 60000ul) / 1000ul);
        }
    }

    printf("   \r");
    fflush(stdout);
}

static void end_progressline() {
    printf("\n");
    progressline_active = 0;
}

static void printf_error(const char *format, ...) {
    va_list ap;

    if (progressline_active) {
        end_progressline();
    }

    va_start(ap, format);
    vfprintf(stderr, format, ap);
    va_end(ap);
}

static int send_request(serport_t hdl, uint8_t *buf, size_t bufsize) {
    uint8_t len;
    fd_set rfds;
    struct timeval tv;
    int r;
    uint8_t status;

    if (bufsize > 0xff) {
        return -1;
    }

    len = (uint8_t)bufsize;

    serport_write(hdl, &len, 1);

    FD_ZERO(&rfds);
    FD_SET(hdl, &rfds);
    tv.tv_sec = 0;
    tv.tv_usec = 100000;
    r = select(hdl + 1, &rfds, NULL, NULL, &tv);
    if (r < 0) {
        printf_error("send_request: select failed\n");
        return -1;
    }
    if (r == 0) {
        printf_error("send_request: timed out waiting for status code\n");
        return -1;
    }
    r = serport_read(hdl, &status, 1);
    if (r <= 0) {
        printf_error("send_request: failed reading status code\n");
        return -1;
    }

    serport_write(hdl, buf, bufsize);

    return 0;
}

/*
 * On function call, *bufsize must contain the number of bytes which may be
 * written to buf; on function return, *bufsize contains the number of bytes
 * actually written
 *
 * Returns:
 *   < 0: internal error or reception timeout
 *     0: ok
 *   > 0: error code as received from SQ610
 */
static int read_reply(serport_t hdl, uint8_t *buf, size_t *bufsize) {
    fd_set rfds;
    struct timeval tv;
    int r;
    uint8_t replen;
    uint8_t recvcount;
    uint8_t ack = 0;

    FD_ZERO(&rfds);
    FD_SET(hdl, &rfds);
    /* Don't set this timeout too small; for example, getting the crc16
       of 64 kB requires more than 400 ms */
    tv.tv_sec = 1;
    tv.tv_usec = 0;
    r = select(hdl + 1, &rfds, NULL, NULL, &tv);
    if (r < 0) {
        printf_error("read_reply: select failed (1)\n");
        return -1;
    }
    if (r == 0) {
        printf_error("read_reply: timed out waiting for length byte\n");
        return -1;
    }
    r = serport_read(hdl, &replen, 1);
    if (r <= 0) {
        printf_error("read_reply: failed reading length byte\n");
        return -1;
    }

    serport_write(hdl, &ack, 1);

    if ((size_t)replen > *bufsize) {
        printf_error("read_reply: reply length (%d) larger than supplied buffer\n", replen);
        return -1;
    }

    recvcount = 0;

    while (recvcount < replen) {
        FD_ZERO(&rfds);
        FD_SET(hdl, &rfds);
        tv.tv_sec = 0;
        tv.tv_usec = 1000000;
        r = select(hdl + 1, &rfds, NULL, NULL, &tv);
        if (r < 0) {
            printf_error("read_reply: select failed (2)\n");
            return -1;
        }
        if (r == 0) {
            printf_error("read_reply: timed out waiting for data byte #%" PRIu8 "\n", recvcount);
            return -1;
        }
        r = serport_read(hdl, buf + recvcount, 1);
        if (r <= 0) {
            printf_error("read_reply: failed reading data byte #%" PRIu8 "\n", recvcount);
            return -1;
        }

        recvcount++;
    }

    *bufsize = (size_t)recvcount;

    return 0;
}

static int cpuinfo(serport_t hdl) {
    uint8_t buf[6];
    size_t bufsize;
    int ret;

    buf[0] = 0x41; /* Command */

    if (send_request(hdl, buf, 1) != 0) {
        printf_error("cpuinfo: send_request failed\n");
        return -1;
    }

    bufsize = sizeof(buf);
    ret = read_reply(hdl, buf, &bufsize);

    if (ret != 0) {
        printf_error("cpuinfo: read_reply failed (error %d)\n", ret);
        return -1;
    }

    if (bufsize != 5) {
        printf_error("cpuinfo: reply contained %zu bytes, expected 5 bytes\n", bufsize);
        return -1;
    }

    printf("cpuinfo:\n");
    printf("0x%02" PRIx8 " (always 0x41/'A')\n", buf[0]);
    printf("0x%02" PRIx8 " (x86 generation?)\n", buf[1]);
    printf("0x%02" PRIx8 "\n", buf[2]);
    printf("0x%02" PRIx8 " (always 0x02)\n", buf[3]);
    printf("0x%02" PRIx8 "\n", buf[4]);

    return 0;
}

static int ioread(serport_t hdl, uint16_t ioaddr, int wordflag) {
    uint8_t buf[4];
    size_t bufsize;
    int ret;

    buf[0] = 0x48; /* Command */
    buf[1] =  ioaddr & 0x00ff;       /* I/O address, LSB */
    buf[2] = (ioaddr & 0xff00) >> 8; /* I/O address, MSB */
    if (wordflag) {
        buf[3] = 0x01;
    } else {
        buf[3] = 0x00;
    }

    if (send_request(hdl, buf, 4) != 0) {
        printf_error("ioread: send_request failed\n");
        return -1;
    }

    bufsize = sizeof(buf);
    ret = read_reply(hdl, buf, &bufsize);

    if (ret != 0) {
        printf_error("ioread: read_reply failed (error %d)\n", ret);
        return -1;
    }

    if (bufsize != 2) {
        printf_error("ioread: reply contained %zu bytes, expected 2 bytes\n", bufsize);
        return -1;
    }

    if (wordflag) {
        printf("ioread: 0x%04" PRIx16 "\n", ((uint16_t)buf[0]) | (((uint16_t)buf[1]) << 8));
    } else {
        printf("ioread: 0x%02" PRIx16 "\n", ((uint16_t)buf[0]) | (((uint16_t)buf[1]) << 8));
    }

    return 0;
}

static int iowrite(serport_t hdl, uint16_t ioaddr, int wordflag, uint16_t data) {
    uint8_t buf[6];
    size_t bufsize;
    int ret;

    buf[0] = 0x49; /* Command */
    buf[1] =  ioaddr & 0x00ff;       /* I/O address, LSB */
    buf[2] = (ioaddr & 0xff00) >> 8; /* I/O address, MSB */
    if (wordflag) {
        buf[3] = 0x01;
    } else {
        buf[3] = 0x00;
    }
    buf[4] =  data & 0x00ff;         /* data, LSB */
    buf[5] = (data & 0xff00) >> 8;   /* data, MSB */

    if (send_request(hdl, buf, 7) != 0) {
        printf_error("iowrite: send_request failed\n");
        return -1;
    }

    bufsize = sizeof(buf);

    ret = read_reply(hdl, buf, &bufsize);

    if (ret != 0) {
        printf_error("iowrite: read_reply failed (error %d)\n", ret);
        return -1;
    }

    if (bufsize != 1) {
        printf_error("iowrite: reply contained %zu bytes, expected 1 bytes\n", bufsize);
        return -1;
    }

    printf("iowrite: response: 0x%02x\n", buf[0]);

    return 0;
}

static int memdump(serport_t hdl, uint16_t addr_segment, uint16_t addr_offset, uint8_t len, uint8_t *data) {
    uint8_t buf[129];
    size_t bufsize;
    int ret;

    if (len > 129) {
        printf_error("memdump: got request to read %" PRIu8 " bytes, but maximum is 129\n", len);
        return -1;
    }

    buf[0] = 0x46; /* Command */
    buf[1] = len;
    buf[2] =  addr_offset & 0x00ff;        /* address offset, LSB */
    buf[3] = (addr_offset & 0xff00) >> 8;  /* address offset, MSB */
    buf[4] =  addr_segment & 0x00ff;       /* address segment, LSB */
    buf[5] = (addr_segment & 0xff00) >> 8; /* address segment, MSB */

    if (send_request(hdl, buf, 6) != 0) {
        printf_error("memdump: send_request failed\n");
        return -1;
    }

    bufsize = sizeof(buf);
    ret = read_reply(hdl, buf, &bufsize);

    if (ret != 0) {
        printf_error("memdump: read_reply failed (error %d)\n", ret);
        return -1;
    }

    if (bufsize != len) {
        printf_error("memdump: reply contained %zu bytes, expected %d bytes\n", bufsize, len);
        return -1;
    }

    if (data != NULL) {
        memcpy(data, buf, len);
    }

    return 0;
}

/* Beware: Any length > 17280 will crash the SQ610?! */
static int crc16(serport_t hdl, uint16_t addr_segment, uint16_t addr_offset, uint16_t length, uint16_t *crc) {
    uint8_t buf[7];
    size_t bufsize;
    int ret;

    buf[0] = 0x22; /* Command */
    buf[1] =  addr_offset & 0x00ff;        /* address offset, LSB */
    buf[2] = (addr_offset & 0xff00) >> 8;  /* address offset, MSB */
    buf[3] =  addr_segment & 0x00ff;       /* address segment, LSB */
    buf[4] = (addr_segment & 0xff00) >> 8; /* address segment, MSB */
    buf[5] =  length & 0x00ff;       /* length, LSB */
    buf[6] = (length & 0xff00) >> 8; /* length, MSB */

    if (send_request(hdl, buf, 7) != 0) {
        printf_error("crc16: send_request failed\n");
        return -1;
    }

    bufsize = sizeof(buf);
    ret = read_reply(hdl, buf, &bufsize);

    if (ret != 0) {
        printf_error("crc16: read_reply failed (error %d)\n", ret);
        return -1;
    }

    if (bufsize != 2) {
        printf_error("crc16: reply contained %zu bytes, expected 2 bytes\n", bufsize);
        return -1;
    }

    *crc = (uint16_t)buf[0] | (((uint16_t)buf[1]) << 8);

    return 0;
}

static int memwrite(serport_t hdl, uint16_t addr_segment, uint16_t addr_offset, uint8_t *data, uint8_t len) {
    uint8_t buf[6 + len];
    size_t bufsize;
    int ret;

    /* Beware: It first seems that up to 249 bytes can be transferred per
       command; however, this corrupts the data of the debug server so that
       it randomly fails to start the transferred program (it only works
       *sometimes*, depending on the payload transferred) */

    if (len > 123) {
        printf_error("memwrite: got request to write %" PRIu8 " bytes, but maximum is 123\n", len);
        return -1;
    }

    buf[0] = 0x47; /* Command */
    buf[1] = len;
    buf[2] =  addr_offset & 0x00ff;        /* address offset, LSB */
    buf[3] = (addr_offset & 0xff00) >> 8;  /* address offset, MSB */
    buf[4] =  addr_segment & 0x00ff;       /* address segment, LSB */
    buf[5] = (addr_segment & 0xff00) >> 8; /* address segment, MSB */
    memcpy(buf + 6, data, len);

    if (send_request(hdl, buf, 6 + len) != 0) {
        printf_error("memwrite: send_request failed\n");
        return -1;
    }

    bufsize = sizeof(buf);
    ret = read_reply(hdl, buf, &bufsize);

    if (ret != 0) {
        printf_error("memwrite: read_reply failed (error %d)\n", ret);
        return -1;
    }

    if (bufsize != 1) {
        printf_error("memwrite: reply contained %zu bytes, expected 1 byte\n", bufsize);
        return -1;
    }

    return 0;
}

static int memset_(serport_t hdl, uint16_t addr_segment, uint16_t addr_offset, uint8_t pattern, uint16_t len) {
    uint8_t buf[9];
    size_t bufsize;
    int ret;

    buf[0]  = 0x51; /* Command */
    buf[1]  =   addr_offset & 0x00ff;       /* address offset, LSB */
    buf[2]  =  (addr_offset & 0xff00) >> 8; /* address offset, MSB */
    buf[3]  =  addr_segment & 0x00ff;       /* address segment, LSB */
    buf[4]  = (addr_segment & 0xff00) >> 8; /* address segment, MSB */
    buf[5]  =           len & 0x00ff;       /* length, LSB */
    buf[6] =           (len & 0xff00) >> 8; /* length, MSB */
    buf[7]  = pattern;                      /* pattern */

    if (send_request(hdl, buf, 9) != 0) {
        printf_error("memset_: send_request failed\n");
        return -1;
    }

    bufsize = sizeof(buf);
    ret = read_reply(hdl, buf, &bufsize);

    if (ret != 0) {
        printf_error("memset_: read_reply failed (error %d)\n", ret);
        return -1;
    }

    if (bufsize != 1) {
        printf_error("memset_: reply contained %zu bytes, expected 1 byte\n", bufsize);
        return -1;
    }

    return 0;
}

static int memcopy(serport_t hdl, uint16_t src_segment, uint16_t src_offset, uint16_t dst_segment, uint16_t dst_offset, uint16_t len) {
    uint8_t buf[11];
    size_t bufsize;
    int ret;

    buf[0]  = 0x52; /* Command */
    buf[1]  =   src_offset & 0x00ff;        /* source address offset, LSB */
    buf[2]  =  (src_offset & 0xff00) >> 8;  /* source address offset, MSB */
    buf[3]  =  src_segment & 0x00ff;        /* source address segment, LSB */
    buf[4]  = (src_segment & 0xff00) >> 8;  /* source address segment, MSB */
    buf[5]  =   dst_offset & 0x00ff;        /* destination address offset, LSB */
    buf[6]  =  (dst_offset & 0xff00) >> 8;  /* destination address offset, MSB */
    buf[7]  =  dst_segment & 0x00ff;        /* destination address segment, LSB */
    buf[8]  = (dst_segment & 0xff00) >> 8;  /* destination address segment, MSB */
    buf[9]  =          len & 0x00ff;        /* length, LSB */
    buf[10] =         (len & 0xff00) >> 8;  /* length, MSB */

    if (send_request(hdl, buf, 11) != 0) {
        printf_error("memcopy: send_request failed\n");
        return -1;
    }

    bufsize = sizeof(buf);
    ret = read_reply(hdl, buf, &bufsize);

    if (ret != 0) {
        printf_error("memcopy: read_reply failed (error %d)\n", ret);
        return -1;
    }

    if (bufsize != 1) {
        printf_error("memcopy: reply contained %zu bytes, expected 1 byte\n", bufsize);
        return -1;
    }

    return 0;
}

static int restore_context(serport_t hdl) {
    uint8_t buf[255];
    size_t bufsize;
    int ret;

    buf[0] = 0x44; /* Command */

    if (send_request(hdl, buf, 1) != 0) {
        printf_error("restore_context: send_request failed\n");
        return -1;
    }

    bufsize = sizeof(buf);
    ret = read_reply(hdl, buf, &bufsize);

    if (ret != 0) {
        printf_error("restore_context: read_reply failed (error %d)\n", ret);
        return -1;
    }

    if (bufsize != 0) {
        printf_error("restore_context: reply contained %zu bytes, expected 0 bytes\n", bufsize);
        return -1;
    }

    return 0;
}

static int fpustateget(serport_t hdl) {
    uint8_t buf[94];
    size_t bufsize;
    int ret;
    int c;

    buf[0] = 0x4c; /* Command */

    if (send_request(hdl, buf, 1) != 0) {
        printf_error("fpustateget: send_request failed\n");
        return -1;
    }

    bufsize = sizeof(buf);
    ret = read_reply(hdl, buf, &bufsize);


    if (ret != 0) {
        printf_error("fpustateget: read_reply failed (error %d)\n", ret);
        return -1;
    }

    if (bufsize != 94) {
        printf_error("fpustateget: reply contained %zu bytes, expected 94 bytes\n", bufsize);
        return -1;
    }

    printf("fpu state: ");
    for (c = 0; c < 94; c++) {
        printf("%02x", buf[c]);
    }
    printf("\n");

    return 0;
}

static int fpustateset(serport_t hdl, uint8_t *data, uint8_t len) {
    uint8_t buf[2 + len];
    size_t bufsize;
    int ret;

    if (len != 94) {
        printf_error("fpustateset: got request to write %" PRIu8 " bytes, but need 94 bytes\n", len);
        return -1;
    }

    buf[0] = 0x4d; /* Command */
    memcpy(buf + 1, data, len);

    if (send_request(hdl, buf, 1 + len) != 0) {
        printf_error("fpustateset: send_request failed\n");
        return -1;
    }

    bufsize = sizeof(buf);
    ret = read_reply(hdl, buf, &bufsize);

    if (ret != 0) {
        printf_error("fpustateset: read_reply failed (error %d)\n", ret);
        return -1;
    }

    if (bufsize != 1) {
        printf_error("fpustateset: reply contained %zu bytes, expected 1 byte\n", bufsize);
        return -1;
    }

    return 0;
}

static int jump(serport_t hdl, uint16_t segment, uint16_t offset) {
    uint8_t buf[2];

    buf[0] =  segment & 0x00ff;
    buf[1] = (segment & 0xff00) >> 8;
    if (memwrite(hdl, 0x0004, 0x0248, buf, 2) != 0) {
        printf_error("memwrite (1) failed\n");
        return -1;
    }

    buf[0] =  offset & 0x00ff;
    buf[1] = (offset & 0xff00) >> 8;
    if (memwrite(hdl, 0x0004, 0x0246, buf, 2) != 0) {
        printf_error("memwrite (2) failed\n");
        return -1;
    }

    if (restore_context(hdl) != 0) {
        printf_error("restore_context failed\n");
        return -1;
    }

    return 0;
}

static int hexcharval(char ch, uint8_t *val) {
    if (ch >= '0' && ch <= '9') {
        *val = ch - '0';
        return 0;
    } else if (ch >= 'a' && ch <= 'f') {
        *val = (ch - 'a') + 10;
        return 0;
    } else if (ch >= 'A' && ch <= 'F') {
        *val = (ch - 'A') + 10;
        return 0;
    }

    return -1;
}

/*
 * On function call, *bufsize must contain the number of bytes which may be
 * written to buf; on function return, *bufsize contains the number of bytes
 * actually written
 */
static int parsehex(char *str, uint8_t *buf, size_t *bufsize) {
    int r;
    uint8_t hi, lo;
    size_t len = 0;

    while (*str != '\0') {
        while (*str == ' ') {
            str++;
        }

        if (*str == '\0') {
            break;
        }

        r = hexcharval(*str, &hi);
        if (r != 0) {
            return -1;
        }
        str++;

        r = hexcharval(*str, &lo);
        if (r != 0) {
            return -1;
        }
        str++;

        if (len == *bufsize) {
            return -1;
        }

        *buf = (hi << 4) | lo;
        buf++;
        len++;
    }

    *bufsize = len;
    return 0;
}

void print_hexdumpline(uint8_t *buf, size_t bytecount) {
    size_t i;

    for (i = 0; i < 16; i++) {
        if (i < bytecount) {
            printf("%02x ", buf[i]);
        } else {
            printf("   ");
        }
    }


    for (i = 0; i < bytecount; i++) {
        if (buf[i] >= 0x20 && buf[i] <= 0x7e) {
            printf("%c", buf[i]);
        } else {
            printf(".");
        }
    }

    printf("\n");
}

void dumpserport(serport_t hdl) {
    fd_set rfds;
    struct timeval tv;
    int r;
    ssize_t readlen, writelen, written;
    uint8_t buf[256];

    for (;;) {
        FD_ZERO(&rfds);
        FD_SET(hdl, &rfds);
        tv.tv_sec = 60;
        tv.tv_usec = 0;
        r = select(hdl + 1, &rfds, NULL, NULL, &tv);
        if (r < 0) {
            printf_error("dumpserport: select failed\n");
            return;
        }
        if (r == 0) {
           printf_error("Timeout dumping serial port\n");
           return;
        }
        readlen = serport_read(hdl, buf, sizeof(buf) - 1);
        if (readlen <= 0) {
            printf_error("dumpserport: read error\n");
            return;
        }
        written = 0;
        while (written < readlen) {
            writelen = write(STDOUT_FILENO, buf + written, readlen - written);
            if (writelen <= 0) {
                printf_error("error writing to stdout?!\n");
                return;
            }
            written += writelen;
        }
        fflush(stdout);
    }
}

static int parse_segment_offset(const char *str, uint16_t *segment, uint16_t *offset) {
    unsigned long int ul;
    char *endptr;

    ul = strtoul(str, &endptr, 0);
    if (endptr != NULL && *endptr != '\0' && *endptr != ':') {
        return -1;
    }
    if (ul > 65535) {
        return -1;
    }
    *segment = (uint16_t)ul;

    if (*endptr == ':') {
        ul = strtoul(endptr + 1, &endptr, 0);

        if (endptr != NULL && *endptr != '\0') {
            return -1;
        }
        if (ul < 0 || ul > 65535) {
            return -1;
        }
        *offset = (uint16_t)ul;
    } else {
        *offset = 0;
    }

    return 0;
}

static int parse_u8(const char *str, uint8_t min, uint8_t max, uint8_t *val) {
    unsigned long int ul;
    char *endptr;

    ul = strtoul(str, &endptr, 0);
    if (endptr != NULL && *endptr != '\0') {
        return -1;
    }
    if (ul < min || ul > max) {
        return -1;
    }

    *val = (uint8_t)ul;

    return 0;
}

static int parse_u16(const char *str, uint16_t min, uint16_t max, uint16_t *val) {
    unsigned long int ul;
    char *endptr;

    ul = strtoul(str, &endptr, 0);
    if (endptr != NULL && *endptr != '\0') {
        return -1;
    }
    if (ul < min || ul > max) {
        return -1;
    }

    *val = (uint16_t)ul;

    return 0;
}

static int docmd(serport_t hdl, const char *cmd, int argc, char *argv[]) {
    int retval = 0;
    int fd = -1;
    uint16_t ioaddr, iodata;
    uint16_t segment, offset, segment2, offset2;
    char *endptr;
    unsigned long int length, curpos, bytecount;
    uint8_t buf[256];
    size_t bufsize;
    uint16_t length16;
    uint16_t crc;
    uint8_t pattern;
    ssize_t transfercount, transfercountsum, writecount;
    struct timeval starttime;
    struct stat statbuf;
    uint16_t divisor;

    if (strcasecmp(cmd, "cpuinfo") == 0) {
        if (argc != 0) {
            printf_error("Syntax: cpuinfo\n");
            goto error;
        }
        if (cpuinfo(hdl) != 0) {
            printf_error("cpuinfo failed\n");
            goto error;
        }
    } else if (strcasecmp(cmd, "ioreadbyte") == 0) {
        if (argc != 1) {
            printf_error("Syntax: ioreadbyte <ioaddr>\n");
            goto error;
        }
        if (parse_u16(argv[0], 0x0000, 0xffff, &ioaddr) != 0) {
            printf_error("Invalid I/O address\n");
            goto error;
        }
        if (ioread(hdl, ioaddr, 0) != 0) {
            printf_error("ioread failed\n");
            goto error;
        }
    } else if (strcasecmp(cmd, "ioreadword") == 0) {
        if (argc != 1) {
            printf_error("Syntax: ioreadword <ioaddr>\n");
            goto error;
        }
        if (parse_u16(argv[0], 0x0000, 0xffff, &ioaddr) != 0) {
            printf_error("Invalid I/O address\n");
            goto error;
        }
        if (ioread(hdl, ioaddr, 1) != 0) {
            printf_error("ioread failed\n");
            goto error;
        }
    } else if (strcasecmp(cmd, "iowritebyte") == 0) {
        if (argc != 2) {
            printf_error("Syntax: iowritebyte <ioaddr> <data>\n");
            goto error;
        }
        if (parse_u16(argv[0], 0x0000, 0xffff, &ioaddr) != 0) {
            printf_error("Invalid I/O address\n");
            goto error;
        }
        if (parse_u16(argv[1], 0x0000, 0xff, &iodata) != 0) {
            printf_error("Invalid data\n");
            goto error;
        }
        if (iowrite(hdl, ioaddr, 0, iodata) != 0) {
            printf_error("iowrite failed\n");
            goto error;
        }
    } else if (strcasecmp(cmd, "iowriteword") == 0) {
        if (argc != 2) {
            printf_error("Syntax: iowriteword <ioaddr> <data>\n");
            goto error;
        }
        if (parse_u16(argv[0], 0x0000, 0xffff, &ioaddr) != 0) {
            printf_error("Invalid I/O address\n");
            goto error;
        }
        if (parse_u16(argv[1], 0x0000, 0xffff, &iodata) != 0) {
            printf_error("Invalid data\n");
            goto error;
        }
        if (iowrite(hdl, ioaddr, 1, iodata) != 0) {
            printf_error("iowrite failed\n");
            goto error;
        }
    } else if (strcasecmp(cmd, "memdump") == 0) {
        if (argc != 2) {
            printf_error("Syntax: memdump <segment/offset> <length>\n");
            goto error;
        }
        if (parse_segment_offset(argv[0], &segment, &offset) != 0) {
            printf_error("Invalid segment/offset\n");
            goto error;
        }
        length = strtoul(argv[1], &endptr, 0);
        if (endptr != NULL && *endptr != '\0') {
            printf_error("Invalid length\n");
            goto error;
        }

        for (curpos = 0; curpos < length; curpos += 0x10) {

            bytecount = length - curpos;
            if (bytecount > 16) {
                bytecount = 16;
            }

            if (memdump(hdl, (uint16_t)(segment + 0x100 * ((2 + curpos) / 0x10000)), (uint16_t)(offset + curpos), bytecount, buf) != 0) {
                printf_error("memdump failed\n");
                goto error;
            }

            printf("0x%04x:0x%04x ", (uint16_t)(segment + 0x100 * ((2 + curpos) / 0x10000)), (uint16_t)(offset + curpos));
            print_hexdumpline(buf, bytecount);
        }
    } else if (strcasecmp(cmd, "crc16") == 0) {
        if (argc != 2) {
            printf_error("Syntax: crc16 <segment/offset> <length>\n");
            goto error;
        }
        if (parse_segment_offset(argv[0], &segment, &offset) != 0) {
            printf_error("Invalid segment/offset\n");
            goto error;
        }
        if (parse_u16(argv[0], 0x0000, 0xffff, &length16) != 0) {
            printf_error("Invalid length\n");
            goto error;
        }
        if (crc16(hdl, segment, offset, length16, &crc) != 0) {
            printf_error("memdump failed\n");
            goto error;
        }
        printf("CRC16: %04" PRIx16 "\n", crc);
    } else if (strcasecmp(cmd, "memwrite") == 0) {
        if (argc != 2) {
            printf_error("Syntax: memwrite <segment/offset> <hexdata...>\n");
            goto error;
        }
        if (parse_segment_offset(argv[0], &segment, &offset) != 0) {
            printf_error("Invalid segment/offset\n");
            goto error;
        }
        bufsize = sizeof(buf);
        if (parsehex(argv[1], buf, &bufsize) != 0) {
            printf_error("Invalid hex data\n");
            goto error;
        }
        if (memwrite(hdl, segment, offset, buf, bufsize) != 0) {
            printf_error("memwrite failed\n");
            goto error;
        }
    } else if (strcasecmp(cmd, "memwritefile") == 0) {
        if (argc != 2) {
            printf_error("Syntax: memwritefile <segment/offset> <filename>\n");
            goto error;
        }
        if (parse_segment_offset(argv[0], &segment, &offset) != 0) {
            printf_error("Invalid segment/offset\n");
            goto error;
        }

        if (stat(argv[1], &statbuf) != 0) {
            printf_error("Failed to stat file '%s'\n", argv[1]);
            goto error;
        }

        fd = open(argv[1], O_RDONLY);
        if (fd < 0) {
            printf_error("Failed to open file '%s' for reading\n", argv[1]);
            goto error;
        }

        transfercountsum = 0;
        if (gettimeofday(&starttime, NULL) != 0) {
            memset(&starttime, 0, sizeof(starttime));
        }

        for (;;) {

            transfercount = read(fd, buf, 123);
            if (transfercount < 0) {
                printf_error("Failed to read from file '%s'\n", argv[1]);
                goto error;
            } else if (transfercount == 0) {
                /* EoF */
                break;
            } else {

                transfercountsum += transfercount;

                if (starttime.tv_sec != 0) {
                    print_progressline(transfercountsum, statbuf.st_size, &starttime);
                }

                while (offset >= 0x100) {
                    offset -= 0x100;
                    segment++;
                }

                if (memwrite(hdl, segment, offset, buf, transfercount) != 0) {
                    printf_error("memwrite failed\n");
                    goto error;
                }

                offset += transfercount;
            }
        }

        if (starttime.tv_sec != 0) {
            end_progressline();
        }

    } else if (strcasecmp(cmd, "memset") == 0) {
        if (argc != 3) {
            printf_error("Syntax: memset <segment/offset> <pattern> <length>\n");
            goto error;
        }
        if (parse_segment_offset(argv[0], &segment, &offset) != 0) {
            printf_error("Invalid segment/offset\n");
            goto error;
        }
        if (parse_u8(argv[1], 0x00, 0xff, &pattern) != 0) {
            printf_error("Invalid pattern\n");
            goto error;
        }
        if (parse_u16(argv[2], 0x0000, 0xffff, &length16) != 0) {
            printf_error("Invalid length\n");
            goto error;
        }
        if (memset_(hdl, segment, offset, pattern, length16) != 0) {
            printf_error("memset_ failed\n");
            goto error;
        }
    } else if (strcasecmp(cmd, "memcopy") == 0) {
        if (argc != 3) {
            printf_error("Syntax: memcopy <source-segment/offset> <destination-segment/offset> <length>\n");
            goto error;
        }
        if (parse_segment_offset(argv[0], &segment, &offset) != 0) {
            printf_error("Invalid source-segment/offset\n");
            goto error;
        }
        if (parse_segment_offset(argv[1], &segment2, &offset2) != 0) {
            printf_error("Invalid destination-segment/offset\n");
            goto error;
        }
        if (parse_u16(argv[2], 0x0000, 0xffff, &length16) != 0) {
            printf_error("Invalid length\n");
            goto error;
        }
        if (memcopy(hdl, segment, offset, segment2, offset2, length16) != 0) {
            printf_error("memcopy failed\n");
            goto error;
        }
    } else if (strcasecmp(cmd, "memdumpfile") == 0) {
        if (argc != 3) {
            printf_error("Syntax: memdumpfile <segment/offset> <length> <filename>\n");
            goto error;
        }
        if (parse_segment_offset(argv[0], &segment, &offset) != 0) {
            printf_error("Invalid segment/offset\n");
            goto error;
        }
        length = strtoul(argv[1], &endptr, 0);
        if (endptr != NULL && *endptr != '\0') {
            printf_error("Invalid length\n");
            goto error;
        }
        fd = open(argv[2], O_WRONLY | O_CREAT | O_TRUNC, 0644);
        if (fd < 0) {
            printf_error("Failed to open file '%s' for writing\n", argv[2]);
            goto error;
        }

        if (gettimeofday(&starttime, NULL) != 0) {
            memset(&starttime, 0, sizeof(starttime));
        }

        transfercountsum = 0;

        for (curpos = 0; curpos < length;) {

            bytecount = length - curpos;
            if (bytecount > 129) {
                bytecount = 129;
            }

            if (memdump(hdl, segment, offset, bytecount, buf) != 0) {
                printf_error("memdump failed\n");
                goto error;
            }

            transfercount = 0;
            while (transfercount < bytecount) {
                writecount = write(fd, buf + transfercount, bytecount - transfercount);
                if (writecount < 0) {
                    printf_error("Write to file failed!\n");
                    goto error;
                }
                transfercount += writecount;
            }

            transfercountsum += transfercount;

            if (starttime.tv_sec != 0) {
                print_progressline(transfercountsum, length, &starttime);
            }

            curpos += bytecount;

            offset += bytecount;
            segment += offset / 0x100;
            offset %= 0x100;
        }

        if (starttime.tv_sec != 0) {
            end_progressline();
        }

    } else if (strcasecmp(cmd, "jump") == 0) {
        if (argc != 1) {
            printf_error("Syntax: jump <segment/offset>\n");
            goto error;
        }
        if (parse_segment_offset(argv[0], &segment, &offset) != 0) {
            printf_error("Invalid segment/offset\n");
            goto error;
        }
        if (jump(hdl, segment, offset) != 0) {
            printf_error("jump failed\n");
            goto error;
        }
        dumpserport(hdl);
    } else if (strcasecmp(cmd, "fpustateget") == 0) {
        if (argc != 0) {
            printf_error("Syntax: fpustateget\n");
            goto error;
        }
        fpustateget(hdl);
    } else if (strcasecmp(cmd, "fpustateset") == 0) {
        if (argc != 1) {
            printf_error("Syntax: fpustateset <hexdata...>\n");
            goto error;
        }
        bufsize = sizeof(buf);
        if (parsehex(argv[0], buf, &bufsize) != 0) {
            printf_error("Invalid hex data\n");
            goto error;
        }
        if (fpustateset(hdl, buf, bufsize) != 0) {
            printf_error("fpustateset failed\n");
            goto error;
        }
    } else if (strcasecmp(cmd, "dumpserport") == 0) {
        dumpserport(hdl);
    } else if (strcasecmp(cmd, "setuart1divisor") == 0) {
        /* For some reason, this does not work (and writing to port
           0xff18 via iowrite doesn't work, too...) */
        if (argc != 1) {
            printf_error("Syntax: setuart1divisor <divisor>\n");
            goto error;
        }
        if (parse_u16(argv[0], 0x0000, 0xffff, &divisor) != 0) {
            printf_error("Invalid divisor\n");
            goto error;
        }
        /* TODO: Put the divisor value into set_uart1_divisor_code */
        if (memwrite(hdl, 0x2000, 0x0000, set_uart1_divisor_code, sizeof(set_uart1_divisor_code)) != 0) {
            printf_error("memwrite failed\n");
            goto error;
        }
        if (jump(hdl, 0x2000, 0x0000) != 0) {
            printf_error("jump failed\n");
            goto error;
        }
    } else {
        printf_error("Invalid command\n");
        goto error;
    }

    goto cleanup;

error:

    retval = -1;

cleanup:

    if (fd >= 0) {
        close(fd);
    }

    return retval;
}

int main(int argc, char *argv[]) {
    int exitval = EXIT_FAILURE;
    int opt;
    serport_t hdl = SERPORT_INVALID;
    char serportdev[256] = "";
    char mode[32];

    if (argc < 2) {
        fprintf(stderr, "Syntax: [-d /dev/tty...] %s <command> [args...]\n", argv[0]);
        goto cleanup;
    }

    while ((opt = getopt(argc, argv, "d:")) != -1) {
        switch (opt) {

        case 'd':
            strncpy(serportdev, optarg, sizeof(serportdev) - 1);
            serportdev[sizeof(serportdev) - 1] = '\0';
            break;

        case '?':
        default:
            fprintf(stderr, "Invalid option(s) specified\n");
            fprintf(stderr, "Syntax: [-d /dev/tty...] %s <command> [args...]\n", argv[0]);
            goto cleanup;
        }
    }

    if (serportdev[0] == '\0') {
        strncpy(serportdev, DEFAULT_SERPORTDEV, sizeof(serportdev) - 1);
        serportdev[sizeof(serportdev) - 1] = '\0';
    }

    printf("Using serial port device %s\n", serportdev);

    hdl = serport_open(serportdev);
    if (hdl == SERPORT_INVALID) {
        printf_error("Failed to open serial port\n");
        goto cleanup;
    }

    strcpy(mode, "115200 8N1");
    if (serport_setmode(hdl, mode) != 0) {
        printf("serport_setmode failed\n");
        goto cleanup;
    }

    /* Read back the mode to make sure it worked */
    if (serport_getmode(hdl, mode, sizeof(mode) - 1) != 0) {
        printf("serport_getmode failed\n");
        goto cleanup;
    }
    mode[sizeof(mode) - 1] = '\0';

    printf("Using serial port mode %s\n", mode);

    if (docmd(hdl, argv[optind], argc - (optind + 1), argv + (optind + 1)) == 0) {
        exitval = EXIT_SUCCESS;
    }

cleanup:

    if (hdl != SERPORT_INVALID) {
        serport_close(hdl);
    }

    exit(exitval);
}

